#include "foc_encoder.h"

uint16 menc15a_absolute_data[2] = {0};
int16  menc15a_absolute_offset_data[2] = {0};
int16  menc15a_speed_data[2] = {0};

uint8 menc15a_init(void)
{
    spi_init(MENC15A_1_SPI, SPI_MODE1, MENC15A_1_SPI_SPEED, MENC15A_1_CLK_PIN, MENC15A_1_MOSI_PIN, MENC15A_1_MISO_PIN, SPI_CS_NULL);    // 閰嶇疆 MENC15A-1 鐨� SPI绔彛
    gpio_init(MENC15A_1_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL);                                                                         // 閰嶇疆 MENC15A-1 鐨� CS绔彛

//    spi_init(MENC15A_2_SPI, SPI_MODE1, MENC15A_2_SPI_SPEED, MENC15A_2_CLK_PIN, MENC15A_2_MOSI_PIN, MENC15A_2_MISO_PIN, SPI_CS_NULL);    // 閰嶇疆 MENC15A-2 鐨� SPI绔彛
//    gpio_init(MENC15A_2_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL);                                                                         // 閰嶇疆 MENC15A-2 鐨� CS绔彛

    return 0;
}

static int32 magnetic_encoder_get_offset (int32 encoder_max, int32 now_location, int32 last_location)
{
    int32 result_data = 0;

    if((encoder_max / 2) < func_abs(now_location - last_location))
    {
        result_data = ((encoder_max / 2) < now_location ? (now_location - encoder_max - last_location) : (now_location + encoder_max - last_location));
    }
    else
    {
        result_data = (now_location - last_location);
    }

    return result_data;
}

uint16 menc15a_get_absolute_data(menc15a_module_enum menc15a_module)
{
    uint16 read_data = 0;

    uint16 data_last = menc15a_absolute_data[menc15a_module];

    if(menc15a_module == menc15a_1_module)
    {
        MENC15A_1_CS(0);

        read_data = spi_read_16bit_register(MENC15A_1_SPI, 0x8021);

        MENC15A_1_CS(1);
    }
    else
    {
        MENC15A_2_CS(0);

        spi_write_8bit(MENC15A_2_SPI, 0x80);
        spi_write_8bit(MENC15A_2_SPI, 0x21);
        read_data = spi_read_8bit(MENC15A_2_SPI);
        read_data = ((read_data << 8) | spi_read_8bit(MENC15A_2_SPI));

        MENC15A_2_CS(1);
    }

    menc15a_absolute_data[menc15a_module] = (read_data & 0x7fff);

    menc15a_absolute_offset_data[menc15a_module] = (int16)magnetic_encoder_get_offset(32768, menc15a_absolute_data[menc15a_module], data_last);

    return menc15a_absolute_data[menc15a_module];
}

int16 menc15a_get_speed_data(menc15a_module_enum menc15a_module)
{
    int16 read_data = 0;

    if(menc15a_module == menc15a_1_module)
    {
        MENC15A_1_CS(0);

        read_data = spi_read_16bit_register(MENC15A_1_SPI, 0x8032);

        MENC15A_1_CS(1);
    }
    else
    {
        MENC15A_2_CS(0);

        read_data = spi_read_16bit_register(MENC15A_2_SPI, 0x8032);

        MENC15A_2_CS(1);
    }

    read_data = (read_data & 0x7fff);

    if(read_data > 16384)
    {
        read_data = read_data - 32768;
    }

    menc15a_speed_data[menc15a_module] = (int16)((float)read_data * 1.917476f);

    return menc15a_speed_data[menc15a_module];
}
